/*
 * @Description: ARES-8015-E-AC伺服电机驱动器，使用MOTECIAN协议
 * @Author: Dryad
 * @Date: 2019-08-01 17:15:45
 */

#pragma once

#include <rtthread.h>
#include "../DeviceDrivers/stm32_drv_can.h"

class Motor_Class
{
public:
    Motor_Class(uint16_t id, bool _cw_flag) : servo_id(id), cw_flag(_cw_flag), can(CAN2) {}
    static void Servo_Init(float max_velocity, float max_rpm); /* 初始化电机线程，计算转速与线速度的转换系数 */

    /* 电机状态定义 */
    enum Motor_State_Enum
    {
        Motor_Disable = 0, /* 电机释放 */
        Motor_Enable = 1,  /* 电机使能 */
        Motor_Stop = 2,    /* 电机根据Pr.88号参数停止运动 */
        Motor_Brake = 3,   /* 电机根据Pr.42和Pr.77急停 */
    };
    static void Set_State(enum Motor_State_Enum value); /* 广播形式设置电机状态 */
    enum Motor_State_Enum Read_State(void) const;       /* 读取电机状态 */
    void Clear_Wrong(void) const;                       /* 清除驱动器故障信息 */
    void Set_Velocity(float velocity) const;            /* 设置车轮线速度 */
    void Set_Velocity(int rpm) const;                   /* 设置电机转速 n/min */

    rt_uint32_t serial_number; /* 驱动器序列号 */
    float current_velocity;    /* 当前线速度 mm/s */

    /* 电机驱动事件定义，单个驱动占用4个事件 */
    enum Motor_Event_Enum
    {
        Init_Completed = 1 << 0,   /* 驱动器初始化完成 */
        Driver_Wrong = 1 << 1,     /* 驱动器报错 */
        Communication_OK = 1 << 2, /* 通信测试通过 */
        Reserve_3 = 1 << 3,        /* 保留事件3 */
    };
    /* 接收电机驱动器事件 */
    static rt_err_t Motor_Event_Recv(enum Motor_Event_Enum event, /* 待接收的事件 */
                                     signed int wait_time,        /* 接收事件等待时间 */
                                     rt_uint8_t option,           /* 接收事件选项 */
                                     int motor = 0);              /* 0表示全部驱动器,1表示第一象限电机,2,3,4同理 */
private:
    const uint16_t servo_id;           /* 从机地址 */
    const bool cw_flag;                /* true表示顺时针为正方向 */
    CAN_TypeDef *const can;            /* CAN通道 */
    static float k_velocity2rpm;       /* 从线速度转换到转速的转换系数,值为max_rpm/max_velocity*/
    enum Motor_State_Enum motor_state; /* 电机状态 */

    enum Driver_CMD_Code : unsigned char
    {
        PRIM_GetSerialNo = 0x08,    /* 获取驱动器序列号 */
        PRIM_EchoTest = 0x0E,       /* 通信测试指令 */
        PRIM_Enable = 0x15,         /* 电机使能 */
        PRIM_Disable = 0x16,        /* 电机释放 */
        PRIM_ClearError = 0x17,     /* 驱动器故障清除 */
        PRIM_EmergencyStop = 0x3B,  /* 紧急停止 */
        PRIM_Stop = 0x3C,           /* 停止运动 */
        PRIM_GetError = 0x3D,       /* 获取驱动器故障信息 */
        PRIM_GetActVelocity = 0x3F, /* 获取电机实际速度 */
        PRIM_SetVelocity = 0x6F,    /* 设置速度控制模式速度设定值 */
        PRIM_GetParam = 0x95,       /* 获取参数表参数 */
        PRIM_SetParam = 0x96,       /* 设置参数表参数 */
        PRIM_SaveParam = 0x97,      /* 保存参数到Flash */
    };                              /* 当前发送指令 */

    /* 驱动器错误代码定义 */
    union Servo_Wrong_Code {
        unsigned short wrong_code;
        struct
        {
            bool fault_system_error : 1;                /* 系统故障 */
            bool warning_i2t_error : 1;                 /* i2t警告 */
            bool fault_parameter_error : 1;             /* 参数错误 */
            bool fault_under_voltage : 1;               /* 欠压报警 */
            bool fault_over_voltage : 1;                /* 过压报警 */
            bool fault_i2t_error : 1;                   /* i2t报警 */
            bool warning_peakcurrent_arrived : 1;       /* 超过峰值电流 */
            bool fault_position_following_error : 1;    /* 位置误差超限 */
            bool fault_encoder : 1;                     /* 编码器故障 */
            bool fault_velocity_following_error : 1;    /* 速度误差超限 */
            bool warning_ipm_temperature : 1;           /* 功率模块温度过高警告 */
            bool fault_ipm_temperature : 1;             /* 功率模块温度过高报警 */
            bool fault_velobserver_following_error : 1; /* 速度超限报警 */
            bool fault_flash_error : 1;                 /* flash故障 */
            bool fault_current_offset : 1;              /* 电流偏差值故障 */
            bool warning_limit_error : 1;               /* 限位警告 */
        };
    } wrong_code; /* 驱动器报警代码 */

    void Init(void);                                   /* 初始化电机 */
    void Parse(const CanRxMsg *const rx_data);         /*解析应答数据*/
    void Set_Motor_State(enum Motor_State_Enum value); /* 设置电机状态 */

    struct rt_thread rx_thread;                   /* 接收应答数据的线程句柄 */
    static void Servo_RX_Thread(void *parameter); /* 接收数据线程 */
    ALIGN(RT_ALIGN_SIZE)                          /* 线程栈4字节对齐 */
    char rx_thread_stack[1024];                   /* 线程栈起始地址 */
    struct rt_messagequeue rx_data_message;       /* 保存驱动器应答数据消息队列 */
    ALIGN(RT_ALIGN_SIZE)
    char rx_data_message_pool[(sizeof(CanRxMsg) + 4) * 16]; /* 消息队列内存，数据大小+消息头指针，最多16个消息 */

    static void Read_Velocity(void *parameter); /* 定期读取速度中断回调函数 */
    static void Read_Wrong(void *parameter);    /* 定期读取错误代码中断回调函数 */
    rt_timer read_velocity_timer, read_wrong_timer;

    static struct rt_event motor_event; /* 电机驱动器事件集合 */
    unsigned char event_shift;          /* 每个驱动器事件偏移量 */

    /* 通信指令格式 */
    union Communication_Data {
        char data[8];
        struct
        {
            char motor_addr;          /* 指令地址 */
            enum Driver_CMD_Code cmd; /* 命令 */
            char para1_high;          /* 参数1高字节 */
            char para1_low;           /* 参数1低字节 */
            char para2_high;          /* 参数2高字节 */
            char para2_low;           /* 参数2低字节 */
            char check_sum_high;      /* 校验位高8位 */
            char check_sum_low;       /* 校验位低8位 */
        };
        rt_uint16_t word_data[4]; /* 用于计算校验 */
    };

    rt_err_t Send(Driver_CMD_Code cmd, unsigned short data1, unsigned short data2) const;            /* 应答形式发送 */
    static rt_err_t Send_Broadcast(Driver_CMD_Code cmd, unsigned short data1, unsigned short data2); /* 广播形式发送 */
    rt_err_t Communication_Test(void) const;                                                         /* 通信测试 */
    static void Motor_Communication_Test_Thread(void *parameter);                                    /* 通信测试线程 */
};
